#include "Segment.h"
#include "Plane.h"
#include "Triangle.h"

#include <cmath>

Segment::Segment(const Point3D& i_a, const Point3D& i_b) : m_a(i_a), m_b(i_b)
  {
  }


Segment::~Segment(void)
  {
  }

Point3D Segment::GetA() const
  {
  return m_a;
  }

Point3D Segment::GetB() const
  {
  return m_b;
  }

Point3D Segment::GetPoint(double i_k) const
  {
  return Point3D(m_a.GetX() * i_k + m_b.GetX() * (1 - i_k),
                 m_a.GetY() * i_k + m_b.GetY() * (1 - i_k),
                 m_a.GetZ() * i_k + m_b.GetZ() * (1 - i_k));
  }

Point3D Segment::GetIntersectionPoint(const Segment& i_other, double i_eps = 1e-8) const
  {
  Plane plane(m_a, m_b, i_other.GetA());
  if (!plane.IsOnPlane(i_other.GetB(), i_eps))
    throw std::string("Segments are not intersect");
  if (fabs(m_a.Dist(m_b) - m_a.Dist(i_other.GetA()) - m_b.Dist(i_other.GetA())) < i_eps ||
      fabs(m_a.Dist(m_b) - m_a.Dist(i_other.GetB()) - m_b.Dist(i_other.GetB())) < i_eps)
    {
    return i_other.GetA();
    }
  if (fabs(m_a.Dist(m_b) - m_a.Dist(i_other.GetB()) - m_b.Dist(i_other.GetB())) < i_eps)
    {
    return i_other.GetB();
    }
  double det = 0, t1 = -2, t2 = -2;
    {
    double a1 = m_b.GetX() - m_a.GetX(), b1 = -i_other.GetB().GetX() + i_other.GetA().GetX(), c1 = i_other.GetA().GetX() - m_a.GetX();
    double a2 = m_b.GetY() - m_a.GetY(), b2 = -i_other.GetB().GetY() + i_other.GetA().GetY(), c2 = i_other.GetA().GetY() - m_a.GetY();
    det = a1 * b2 - a2 * b1;
    if (fabs(det) > i_eps)
      {
      t1 = (c1 * b2 - c2 * b1) / det;
      t2 = (a1 * c2 - a2 * c1) / det;
      }
    }
  if (fabs(det) < i_eps)
    {
    double a1 = m_b.GetX() - m_a.GetX(), b1 = -i_other.GetB().GetX() + i_other.GetA().GetX(), c1 = i_other.GetA().GetX() - m_a.GetX();
    double a2 = m_b.GetZ() - m_a.GetZ(), b2 = -i_other.GetB().GetZ() + i_other.GetA().GetZ(), c2 = i_other.GetA().GetZ() - m_a.GetZ();
    det = a1 * b2 - a2 * b1;
    if (fabs(det) > i_eps)
      {
      t1 = (c1 * b2 - c2 * b1) / det;
      t2 = (a1 * c2 - a2 * c1) / det;
      }
    }
  if (fabs(det) < i_eps)
    {
    double a1 = m_b.GetY() - m_a.GetY(), b1 = -i_other.GetB().GetY() + i_other.GetA().GetY(), c1 = i_other.GetA().GetY() - m_a.GetY();
    double a2 = m_b.GetZ() - m_a.GetZ(), b2 = -i_other.GetB().GetZ() + i_other.GetA().GetZ(), c2 = i_other.GetA().GetZ() - m_a.GetZ();
    det = a1 * b2 - a2 * b1;
    if (fabs(det) > i_eps)
      {
      t1 = (c1 * b2 - c2 * b1) / det;
      t2 = (a1 * c2 - a2 * c1) / det;
      }
    else
      {
      throw std::string("Segments are not intersect");
      }
    }
  if (-i_eps < t1 && t1 < 1 + i_eps &&
      -i_eps < t2 && t2 < 1 + i_eps)
    {
    if (fabs(m_a.GetX() + t1 * (m_b.GetX() - m_a.GetX()) - (i_other.GetA().GetX() + t2 * (i_other.GetB().GetX() - i_other.GetA().GetX()))) < i_eps &&
        fabs(m_a.GetY() + t1 * (m_b.GetY() - m_a.GetY()) - (i_other.GetA().GetY() + t2 * (i_other.GetB().GetY() - i_other.GetA().GetY()))) < i_eps && 
        fabs(m_a.GetZ() + t1 * (m_b.GetZ() - m_a.GetZ()) - (i_other.GetA().GetZ() + t2 * (i_other.GetB().GetZ() - i_other.GetA().GetZ()))) < i_eps)
      return Point3D(m_a.GetX() + t1 * (m_b.GetX() - m_a.GetX()), m_a.GetY() + t1 * (m_b.GetY() - m_a.GetY()), m_a.GetZ() + t1 * (m_b.GetZ() - m_a.GetZ()));
    }
  throw std::string("Segments are not intersect");
  }

bool Segment::IsIntersect(const Segment& i_other, double i_eps) const
  {
  try
    {
    GetIntersectionPoint(i_other, i_eps);
    }
  catch(const std::string&)
    {
    }
  return false;
  }